// Use IMU to track direction

// Set Local map for obstacle avoidance

// Use IMU to find way point

// IR data filter and tracking

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/GridCells.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>

#include <string>
#include <vector>

#define MAP_WIDTH 10   		// perpendicular to the moving direction of robot
#define MAP_HEIGHT 10
#define MAP_SIZE 500
#define MAP_RESOLUTION 0.01	// meter
#define LASER_RANGE 5.0 	// meter
#define IR_NUMBER 5
#define IR_MAX_VALUE 10000.0
#define IR_MIN_VALUE 0.0
#define IR_MAX_RANGE 2.0	// meter
#define IR_MIN_RANGE 0.0	// meter
#define INFLATION_WIDTH 0.1	// meter
#define PI 3.14159
ros::Subscriber ir_data;
ros::Publisher obs_map_pub;
ros::Publisher obs_map_pub_1;
ros::Publisher occup_grid_pub;

std::string map_frame;

bool obs_map[MAP_SIZE][MAP_SIZE];
double ir_x[5];

double cur_direct, des_direct;

void init();
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
void irCallback(const sensor_msgs::Range::ConstPtr &msg);

void add_obstacle(double x, double y);
void searchPath(bool map[MAP_SIZE][MAP_SIZE]);
//void getRange(int x1, int x2, int* low, int* up);

int main(int argc, char **argv) {

	ros::init(argc, argv, "obstacle_avoidance");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");

	pn.param("obs_map_frame", map_frame, std::string("laser"));
	ros::Subscriber laser_sub = n.subscribe<sensor_msgs::LaserScan> ("/scan",
			50, scanCallback);
	obs_map_pub = n.advertise<nav_msgs::GridCells> ("obstaclemap", 1);
	obs_map_pub_1 = n.advertise<nav_msgs::GridCells> ("obstaclemap_1", 1);

	ir_data = n.subscribe("IRSensor", 1000, irCallback);

	cur_direct = PI / 2;
	des_direct = PI / 2;

	ros::spin();

	return 0;
}

void init() {
	for (int i = 0; i < MAP_SIZE; i++)
		for (int j = 0; j < MAP_SIZE; j++)
			obs_map[i][j] = 0;

	double left_ir_offset = -0.1;
	double ir_interval = 0.05;
	for (int i = 0; i < IR_NUMBER; i++) {
		ir_x[i] = left_ir_offset + ir_interval * i;
	}
}

void irCallback(const sensor_msgs::Range::ConstPtr &msg) {
	//ir_left_bottom, ir_left_top, ir_center, ir_right_bottom, ir_right_top
	int id = msg->radiation_type;
	double dist = msg->range;
	// convert range to meter value
	dist = (dist - IR_MIN_VALUE) / (IR_MAX_VALUE - IR_MIN_VALUE)
			* (IR_MAX_RANGE - IR_MIN_RANGE) + IR_MIN_RANGE;

	if (dist < IR_MAX_RANGE)
		add_obstacle(ir_x[id], dist);
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
	double x_origin = 0.0;
	double y_origin = 0.0;
	nav_msgs::GridCells obs_grid;
	obs_grid.header.frame_id = map_frame;
	obs_grid.header.stamp = ros::Time();
	obs_grid.cell_width = MAP_RESOLUTION;
	obs_grid.cell_height = MAP_RESOLUTION;

	// filter scan data

	// Notice the difference of the angle between SCAN and Obstacle map
	// In SCAN message, x is point ahead, which will cause x, y below zero
	// However, the coordinates of map should be positive(2-D array)
	// So, for convenience, make x be the y axis, y be the x axis
	// Ignore the angle bigger than 90, cause Kinect only has 57 degree
	int x_o = (int) (MAP_SIZE / 2), y_o = 0;

	for (int i = 0; i < MAP_SIZE; i++)
		for (int j = 0; j < MAP_SIZE; j++)
			obs_map[i][j] = 0;

	for (int i = 0; i < scan->ranges.size(); i++) {
		double range = scan->ranges[i];
		// get out extreme value
		if (i > 0 && i < scan->ranges.size())
			range = (range + scan->ranges[i - 1] + scan->ranges[i + 1]) / 3.0;
		double origin_angle = (double) i * scan->angle_increment + scan->angle_min;
		if (std::abs(origin_angle) > PI / 2 || range >= LASER_RANGE)
			continue;
		double angle = origin_angle + PI / 2;
		// find the nearest point, using ray tracing to set obstacle
		double x = range * cos(angle);
		double y = range * sin(angle);

		int x_ind = (int) (x / MAP_RESOLUTION) + x_o, y_ind = (int) (y
				/ MAP_RESOLUTION) + y_o;
		if (x_ind < 0 || y_ind < 0 || x_ind >= MAP_SIZE || y_ind >= MAP_SIZE)
			continue;

		//int d_y = 1, d_x = x_ind < x_o ? -1 : 1,
		int now_x = x_ind, now_y = y_ind;
		double slope = std::abs(angle - PI / 2) < 0.00001 ? -1 : sin(angle) / cos(angle);


		if (std::abs(angle - PI / 2) < 0.00001) {
			for (int i = y_ind; i < MAP_SIZE; i++)
				obs_map[i][now_x] = true;
			//ROS_INFO("%.2lf %.2lf %.2lf %.2lf %.2lf %.2lf", x, y, origin_angle, angle, std::abs(angle - PI / 2), sin(angle));
		} else {
			double slope = sin(angle) / cos(angle);
			if (slope >= 0) {
				for (int i = x_ind; i < MAP_SIZE; i++)
					if ((int) std::abs(slope * (i - x_o)) >= 0 && (int) std::abs(slope
							* (i - x_o)) < MAP_SIZE)
						obs_map[(int) std::abs(slope * (i - x_o))][i] = true;
			} else {
				for (int i = x_ind; i >= 0; i--)
					if ((int) std::abs(slope * (i - x_o)) >= 0 && (int) std::abs(slope
							* (i - x_o)) < MAP_SIZE)
						obs_map[(int) std::abs(slope * (i - x_o))][i] = true;
			}
		}
	}

	std::vector<geometry_msgs::Point> obstacle_seq;
	for (int i = 0; i < MAP_SIZE; i++)
		for (int j = 0; j < MAP_SIZE; j++) {
			if (obs_map[i][j]) {
				geometry_msgs::Point a;
				a.x = i * MAP_RESOLUTION;
				a.y = -(j - x_o) * MAP_RESOLUTION;
				a.z = 0.0;
				obstacle_seq.push_back(a);
			}
		}

	obs_grid.cells.resize(obstacle_seq.size());
	int obstacle_counter = 0;
	while (!obstacle_seq.empty()) {
		obs_grid.cells[obstacle_counter++] = obstacle_seq.back();
		obstacle_seq.pop_back();
	}
	obs_map_pub.publish(obs_grid);

	//searchPath(obs_map);
	//ROS_INFO("Angle %lf", cur_direct);

	// Compare correct grid cell
	nav_msgs::GridCells obs_grid_1;
	obs_grid_1.header.frame_id = map_frame;
	obs_grid_1.header.stamp = ros::Time();
	obs_grid_1.cell_width = MAP_RESOLUTION;
	obs_grid_1.cell_height = MAP_RESOLUTION;
	std::vector<geometry_msgs::Point> obstacle_seq_1;
	for (int i = 0; i < scan->ranges.size(); i++) {
		double range = scan->ranges[i];
		double angle = ((double) i * scan->angle_increment) + scan->angle_min;
		geometry_msgs::Point a;
		a.x = range * cos(angle);
		a.y = range * sin(angle);
		a.z = 0.0;
		obstacle_seq_1.push_back(a);
	}
	obs_grid_1.cells.resize(obstacle_seq_1.size());
	int obstacle_counter_1 = 0;
	while (!obstacle_seq_1.empty()) {
		obs_grid_1.cells[obstacle_counter_1++] = obstacle_seq_1.back();
		obstacle_seq_1.pop_back();
	}
	obs_map_pub_1.publish(obs_grid_1);
}

void add_obstacle(double x, double y) {

}
void getRange(int x1, int x2, int* low, int* up) {
	if (x1 > x2) {
		*low = x2;
		*up = x1;
	} else {
		*low = x1;
		*up = x2;
	}
}
void searchPath(bool map[MAP_SIZE][MAP_SIZE]) {
	double angle_step = 0.1, angle_incre = 0.0, best_angle = -1;
	double turn_angle_bound = 30 / 180 * PI;
	// angle on the right side of PI / 2
	double max_right_angle = cur_direct - turn_angle_bound;
	double max_left_angle = cur_direct + turn_angle_bound;
	int best_dist = 0, required_dist = MAP_SIZE / 2;
	// search ahead
	int now_x = (int) (MAP_SIZE / 2), now_y = 0;
	int valid_dist = 0;
	while (++now_y < MAP_SIZE) {
		if (!map[now_y][now_x])
			valid_dist++;
		else
			break;
	}
	if (valid_dist >= required_dist) {
		cur_direct = PI / 2;
		return;
	}
	best_dist = valid_dist;
	//ROS_INFO("dist of PI / 2 %d", best_dist);

	// search right side
	double now_angle = max_right_angle;
	while (now_angle < PI / 2) {
		int now_x = (int) (MAP_SIZE / 2), now_y = 1;
		int valid_dist = 0;
		while (++now_y < MAP_SIZE && (int) ((double) now_y / tan(now_angle)) >= 0 && (int) ((double) now_y / tan(now_angle)) < MAP_SIZE) {
			if (!map[now_y][(int) ((double) now_y / tan(now_angle))])
				valid_dist++;
			else
				break;
		}
		if (valid_dist >= required_dist) {
			cur_direct = now_angle;
			return;
		}
		best_dist = best_dist > valid_dist ? best_dist : valid_dist;
		best_angle = best_dist == valid_dist ? now_angle : best_angle;
		now_angle += angle_step;

		//ROS_INFO("dist of right %lf %d", now_angle, best_dist);
	}

	// search left side
	now_angle = max_left_angle;
	while (now_angle < PI / 2) {
		int now_x = (int) (MAP_SIZE / 2), now_y = 1;
		int valid_dist = 0;
		while (++now_y < MAP_SIZE && (int) ((double) now_y / tan(now_angle)) >= 0 && (int) ((double) now_y / tan(now_angle)) < MAP_SIZE) {
			if (!map[now_y][(int) ((double) now_y / tan(now_angle))])
				valid_dist++;
			else
				break;
		}
		if (valid_dist >= required_dist) {
			cur_direct = now_angle;
			return;
		}
		best_dist = best_dist > valid_dist ? best_dist : valid_dist;
		best_angle = best_dist == valid_dist ? now_angle : best_angle;
		now_angle -= angle_step;

		//ROS_INFO("dist of right %lf %d", now_angle, best_dist);
	}
	cur_direct = best_angle;
}

